/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.model;

import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.api.MotorSpeeds;
import com.team1160.breakaway.ballControl.Dribbler;
import com.team1160.breakaway.ballControl.Kicker;
import com.team1160.breakaway.drive.Drive;
import com.team1160.breakaway.util.MotorUtil;
import edu.wpi.first.wpilibj.Accelerometer;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.camera.AxisCamera;

/**
 * Model of the robot and sub-components.
 * A source of the robot state.
 * May also receive commands to set the state.
 *
 * @author nttoole
 */
public class TheRobot {

    //six motors
    Drive drive;

    Dribbler dribbler;
    Kicker   kicker;

    //one camera
    AxisCamera camera;

    //one acel
    Accelerometer accelerometer;

    //one gyro
    Gyro gyro;

    //---------------------------------------------------------------------

    public TheRobot()
    {
        init();
    }

    //---------------------------------------------------------------------

    protected void init()
    {
        buildRobot();
    }

    //---------------------------------------------------------------------

    protected void buildRobot()
    {
        //create the motors
        this.drive = new Drive();

        //init the dribbler
        this.dribbler = new Dribbler();

        //init the kicker
        this.kicker = new Kicker();

        //get the camera
        this.camera = AxisCamera.getInstance();

        //create the gyro
        this.gyro = new Gyro(Constants.C_GYRO);

        //create the accelerometer
        this.accelerometer = new Accelerometer(Constants.C_ACCELEROMETER);

    }

    //---------------------------------------------------------------------

    public RobotState getCurrentState()
    {
        RobotState state = new RobotState();

        double dValue;
        boolean bValue;

        //kicker state
        state.setKickerReady(kicker.isRetracted());

        //accelerometer
        dValue = this.accelerometer.getAcceleration();
        state.setAccelerometerVal(dValue);

        //dribble
        dValue = this.accelerometer.getAcceleration();
        bValue = MotorUtil.isMotorStopped(dValue);
        state.setDribblerActive(!bValue);

        //gyro
        dValue = this.gyro.getAngle();
        state.setGyroAngle(dValue);

        //TODO : get drive state
        double lfMot, rfMot, lrMot, rrMot;
        lfMot = 0.0;
        rfMot = 0.0;
        lrMot = 0.0;
        rrMot = 0.0;
        MotorSpeeds speeds = new MotorSpeeds(lfMot, rfMot, lrMot, rrMot);
        state.setMotorSpeeds(speeds);

        return state;
    }


    //---------------------------------------------------------------------


    //---------------------------------------------------------------------


    //---------------------------------------------------------------------

}
